
#include <pthread.h>
#include <stdio.h>
#include <errno.h>
#include <unistd.h>
#include <string.h>
#include "ModbusProt.h"

#define MODBUS_TCP_DEV_DEMO_IP   "192.168.1.200"
#define MODBUS_TCP_DEV_DEMO_PORT 502

/**
 * @brief modbus tcp 读取数据
 * @param ptModbus modbus使用参数结构体指针
 * @param ptMbTable modbus tcp 点表
 * @param iTableSize 点表大小
 * @param iStartAddr 起始地址
 * @param pData 数据指针
 * @return -1failed, other success
 */
static int _modbus_tcp_read(TModbus *ptModbus, TModbusTable *ptMbTable, int iTableSize, int iStartAddr, void *pData)
{
    int iRet = -1;
    if (ptModbus == NULL || ptModbus->Modbus == NULL || ptMbTable == NULL) {
        return e_ModbusRtuErrPtrNull;
    }
    MODBUS_LOCK(&ptModbus->Lock);
    /* 查询点表中是否存在该地址 */
    for (int i = 0; i < iTableSize; i++) {
        if (ptMbTable[i].StartAddr == iStartAddr) {
            for (int j = 0; j < MODBUS_RESEND_CNT; j++) {
                switch (ptMbTable[i].FunctionCode) {
                    case MODBUS_FC_READ_COILS:
                        iRet = modbus_read_bits(ptModbus->Modbus, ptMbTable[i].StartAddr, ptMbTable[i].RegNumber, (uint8_t *)pData);
                        break;
                    case MODBUS_FC_READ_DISCRETE_INPUTS:
                        iRet = modbus_read_input_bits(ptModbus->Modbus, ptMbTable[i].StartAddr, ptMbTable[i].RegNumber, (uint8_t *)pData);
                        break;
                    case MODBUS_FC_READ_HOLDING_REGISTERS:
                        iRet = modbus_read_registers(ptModbus->Modbus, ptMbTable[i].StartAddr, ptMbTable[i].RegNumber, (uint16_t *)pData);
                        break;
                    case MODBUS_FC_READ_INPUT_REGISTERS:
                        iRet = modbus_read_input_registers(ptModbus->Modbus, ptMbTable[i].StartAddr, ptMbTable[i].RegNumber, (uint16_t *)pData);
                        break;
                    default:
                        break;
                }
                if (iRet >= 0) {
                    ptModbus->IsComm = true;
                    break;
                }
            }
            break;
        }
    }
    MODBUS_UNLOCK(&ptModbus->Lock);
    return iRet;
}

/**
 * @brief modbus tcp 写入数据
 * @param ptModbus modbus使用参数结构体指针
 * @param ptMbTable modbus tcp 点表
 * @param iTableSize 点表大小
 * @param iStartAddr 起始地址
 * @param pData 数据指针
 * @return -1failed, other success
 */
static int _modbus_tcp_write(TModbus *ptModbus, TModbusTable *ptMbTable, int iTableSize, int iStartAddr, void *pData)
{
    int iRet = -1;
    if (ptModbus == NULL || ptModbus->Modbus == NULL || ptMbTable == NULL) {
        return e_ModbusRtuErrPtrNull;
    }
    MODBUS_LOCK(&ptModbus->Lock);
    /* 查询点表中是否存在该地址 */
    for (int i = 0; i < iTableSize; i++) {
        if (ptMbTable[i].StartAddr == iStartAddr) {
            for (int j = 0; j < MODBUS_RESEND_CNT; j++) {
                switch (ptMbTable[i].FunctionCode) {
                    case MODBUS_FC_WRITE_SINGLE_COIL:
                        iRet = modbus_write_bit(ptModbus->Modbus, ptMbTable[i].StartAddr, *((uint8_t *)pData));
                        break;
                    case MODBUS_FC_WRITE_SINGLE_REGISTER:
                        iRet = modbus_write_register(ptModbus->Modbus, ptMbTable[i].StartAddr, *((uint16_t *)pData));
                        break;
                    case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
                        iRet = modbus_write_registers(ptModbus->Modbus, ptMbTable[i].StartAddr, ptMbTable[i].RegNumber, (uint16_t *)pData);
                        break;
                    default:
                        break;
                }
                if (iRet >= 0) {
                    ptModbus->IsComm = true;
                    break;
                }
            }
            break;
        }
    }
    MODBUS_UNLOCK(&ptModbus->Lock);
    return iRet;
}

/**
 * @brief modbus tcp 重连函数
 * @param ptModbus modbus使用参数结构体指针
 */
static void _modbus_tcp_reconnect(TModbus *ptModbus)
{
    MODBUS_LOCK(&ptModbus->Lock);
    ptModbus->IsComm = false;
    /* 如果当前已经处于连接状态，则断开 */
    if (ptModbus->IsConnect) {
        ptModbus->Onclose(ptModbus->Modbus);
    }
    /* 连接从机，如果连接失败 */
    if (ptModbus->OnConnect(ptModbus->Modbus) == -1) {
        ptModbus->IsConnect = false;
    }
    /* 连接成功 */
    else {
        ptModbus->IsConnect = true;
    }
    MODBUS_UNLOCK(&ptModbus->Lock);
    return;
}

/**
 * @brief 处理 modbus tcp 数据
 * @param ptModbus TModbus *
 * @param pu8ModbusAdu modbus adu
 * @return -1 fail
 */
static int _modbus_tcp_deal_data(TModbus *ptModbus, uint8_t *pu8ModbusAdu)
{
    int iRet = 0;
    /* 功能码 */
    uint8_t u8FunctionCode = pu8ModbusAdu[7];
    /* 寄存器地址 */
    uint16_t u16RegAddr = (pu8ModbusAdu[8] << 8) | pu8ModbusAdu[9];

    switch (u8FunctionCode) {
        case MODBUS_FC_READ_HOLDING_REGISTERS:    ///< 主站读取从站
        {
            /* 心跳计数 */
            ptModbus->HearBeat++;
            /* 寄存器数量 */
            uint16_t u16RegNumber = (pu8ModbusAdu[10] << 8) | pu8ModbusAdu[11];
            for (int i = 0; i < u16RegNumber; i++) {
                /* 从站数据 --> 主站数据 */
                ptModbus->Map->tab_registers[i + u16RegAddr] = ptModbus->Data[i + u16RegAddr];
            }
            break;
        }
        case MODBUS_FC_WRITE_SINGLE_REGISTER:    ///< 主站写入从站
        {
            /* 主站数据 --> 从站数据 */
            ptModbus->Map->tab_registers[u16RegAddr] = (pu8ModbusAdu[10] << 8) | pu8ModbusAdu[11];
            ptModbus->Data[u16RegAddr] = (pu8ModbusAdu[10] << 8) | pu8ModbusAdu[11];
            break;
        }
        default:
            iRet = -1;
            break;
    }

    return iRet;
}

/**
 * @brief 线程 - modbus tcp 从站
 * @param arg 传入参数
 * @return 无
 */
static void *_thread_modbus_tcp_slave(void *arg)
{
    TModbus *ptModbus = (TModbus *)arg;
    uint8_t  sRecvData[MODBUS_TCP_MAX_ADU_LENGTH] = { 0 };
    int      iRet = -1;
    int      uFdNum = -1;
    int      iFdMax = ptModbus->Socket;
    fd_set   tFdReads;
    fd_set   tFdCpyeads;
    FD_ZERO(&tFdReads);
    FD_SET(ptModbus->Socket, &tFdReads);

    while (1) {
        MODBUS_LOCK(&ptModbus->Lock);
        tFdCpyeads = tFdReads;
        uFdNum = select(iFdMax + 1, &tFdCpyeads, 0, 0, 0);
        if (uFdNum == -1
            || uFdNum == 0) {
            MODBUS_UNLOCK(&ptModbus->Lock);
            continue;
        }

        for (int i = 0; i < iFdMax + 1; i++) {
            if (!FD_ISSET(i, &tFdCpyeads)) {
                MODBUS_UNLOCK(&ptModbus->Lock);
                continue;
            }

            if (i == ptModbus->Socket) {
                int iNsocket = modbus_tcp_accept(ptModbus->Modbus, &ptModbus->Socket);
                if (iNsocket == -1) {
                    MODBUS_UNLOCK(&ptModbus->Lock);
                    continue;
                }

                FD_SET(iNsocket, &tFdReads);
                if (iFdMax < iNsocket) {
                    iFdMax = iNsocket;
                }
            }
            else {
                /* 此功能可用于管理多个客户端 连接到同一服务器 */
                modbus_set_socket(ptModbus->Modbus, i);

                iRet = modbus_receive(ptModbus->Modbus, sRecvData);
                if (iRet == 0) {
                    /* code */
                }
                else if (iRet > 0) {
                    _modbus_tcp_deal_data(ptModbus, sRecvData);
                    modbus_reply(ptModbus->Modbus, sRecvData, iRet, ptModbus->Map);
                }
                else {
                    FD_CLR(i, &tFdReads);
                }

                modbus_set_socket(ptModbus->Modbus, ptModbus->Socket);
            }
        }
        MODBUS_UNLOCK(&ptModbus->Lock);
        usleep(1000 * 10);
    }
    return NULL;
}

/**
 * @brief 创建 modbus tcp 从站对象
 * @param tModbusTcpCfg modbus tcp 配置
 * @param iPriority 线程优先级
 * @return TModbus *
 */
TModbus *modbus_tcp_slave_create(TModbusTcpCfg tModbusTcpCfg, int iPriority)
{
    int            iRet = 0;
    pthread_attr_t tAttr;
    TModbus       *ptModbus = (TModbus *)malloc(sizeof(TModbus));
    if (ptModbus) {
        iRet = pthread_mutex_init(&ptModbus->Lock, NULL);
        if (iRet != 0) {
            printf("%s >> pthread_mutex_init failed\n", __func__);
            free(ptModbus);
            ptModbus = NULL;
            return NULL;
        }

        /* 设置TCP信息 */
        ptModbus->Modbus = modbus_new_tcp(tModbusTcpCfg.IpAddr, tModbusTcpCfg.Port);
        if (ptModbus->Modbus == NULL) {
            printf("%s >> modbus_new_tcp failed, Err: %s\n", __func__, modbus_strerror(errno));
            free(ptModbus);
            ptModbus = NULL;
            return NULL;
        }

        /* 是否设置调试模式 */
        iRet = modbus_set_debug(ptModbus->Modbus, tModbusTcpCfg.IsDebug);
        if (iRet == -1) {
            printf("%s >> modbus_set_debug failed, Err: %s\n", __func__, modbus_strerror(errno));
            modbus_free(ptModbus->Modbus);
            free(ptModbus);
            ptModbus = NULL;
            return NULL;
        }

        /* 判断映射数据大小是否为0 */
        if (tModbusTcpCfg.DataSize == 0) {
            printf("%s >> data_size is 0\n", __func__);
            modbus_free(ptModbus->Modbus);
            free(ptModbus);
            ptModbus = NULL;
            return NULL;
        }

        /* 申请数据内存映射区的堆空间 */
        ptModbus->Data = (uint16_t *)malloc(sizeof(uint16_t) * tModbusTcpCfg.DataSize);
        if (ptModbus->Data == NULL) {
            printf("%s >> malloc iData failed\n", __func__);
            modbus_free(ptModbus->Modbus);
            free(ptModbus);
            ptModbus = NULL;
            return NULL;
        }
        else {
            /* 空间清零 */
            memset(ptModbus->Data, 0, sizeof(uint16_t) * tModbusTcpCfg.DataSize);
        }

        /* 内存映射 */
        ptModbus->Map = modbus_mapping_new_start_address(0, 0, 0, 0,
                                                         /* 保持寄存器起始地址  保持寄存器数量  输入寄存器起始地址  输入寄存器数量 */
                                                         0, tModbusTcpCfg.DataSize, 0, 0);
        if (ptModbus->Map == NULL) {
            printf("%s >> modbus_mapping_new_start_address failed, Err: %s\n", __func__, modbus_strerror(errno));
            modbus_free(ptModbus->Modbus);
            free(ptModbus);
            ptModbus = NULL;
            return NULL;
        }

        /* 监听套接字 */
        ptModbus->Socket = modbus_tcp_listen(ptModbus->Modbus, 20);    ///< 20 - 最大监听数量
        if (ptModbus->Socket == -1) {
            printf("%s >> modbus_tcp_listen failed, Err: %s\n", __func__, modbus_strerror(errno));
            modbus_free(ptModbus->Modbus);
            free(ptModbus);
            ptModbus = NULL;
            return NULL;
        }

        /* 初始化线程属性 */
        pthread_attr_init(&tAttr);
        /* 设置线程调度策略 - SCHED_RR - 时间片轮询调度 */
        iRet = pthread_attr_setschedpolicy(&tAttr, SCHED_RR);
        if (iRet != 0) {
            printf("%s >> failed to pthread_attr_setschedpolicy", __func__);
            modbus_free(ptModbus->Modbus);
            free(ptModbus);
            ptModbus = NULL;
            return NULL;
        }
        /* 设置线程优先级（数值越大，优先级越高） */
        struct sched_param tSched;
        tSched.sched_priority = iPriority;
        iRet = pthread_attr_setschedparam(&tAttr, &tSched);
        if (iRet != 0) {
            printf("%s >> failed to pthread_attr_setschedparam", __func__);
            modbus_free(ptModbus->Modbus);
            free(ptModbus);
            ptModbus = NULL;
            return NULL;
        }

        /* 创建从站线程 */
        iRet = pthread_create(&ptModbus->SlaveThreadId, &tAttr, _thread_modbus_tcp_slave, ptModbus);
        if (iRet != 0) {
            printf("%s >> pthread_create failed\n", __func__);
            modbus_free(ptModbus->Modbus);
            free(ptModbus);
            ptModbus = NULL;
            return NULL;
        }
    }
    return ptModbus;
}

/**
 * @brief modbus tcp 主站初始化
 * @param tModbusTcpCfg modbus tcp 配置
 * @return TModbus *
 */
TModbus *modbus_tcp_master_create(TModbusTcpCfg tModbusTcpCfg)
{
    int      iRet = 0;
    TModbus *ptModbus = (TModbus *)malloc(sizeof(TModbus));
    if (ptModbus) {
        memset(ptModbus, 0, sizeof(*ptModbus));
        iRet = pthread_mutex_init(&ptModbus->Lock, NULL);
        if (iRet != 0) {
            printf("%s >> pthread_mutex_init failed\n", __func__);
            free(ptModbus);
            ptModbus = NULL;
            return NULL;
        }

        /* 设置TCP信息 */
        ptModbus->Modbus = modbus_new_tcp(tModbusTcpCfg.IpAddr, tModbusTcpCfg.Port);
        if (ptModbus->Modbus == NULL) {
            printf("%s >> modbus_new_tcp failed, Err: %s\n", __func__, modbus_strerror(errno));
            free(ptModbus);
            ptModbus = NULL;
            return NULL;
        }

        /* 是否设置调试模式 */
        iRet = modbus_set_debug(ptModbus->Modbus, tModbusTcpCfg.IsDebug);
        if (iRet == -1) {
            printf("%s >> modbus_set_debug failed, Err: %s\n", __func__, modbus_strerror(errno));
            modbus_free(ptModbus->Modbus);
            free(ptModbus);
            ptModbus = NULL;
            return NULL;
        }

        /* 设置设备ID */
        iRet = modbus_set_slave(ptModbus->Modbus, tModbusTcpCfg.DevId);
        if (iRet == -1) {
            printf("%s >> modbus_set_slave failed, Err: %s\n", __func__, modbus_strerror(errno));
            modbus_free(ptModbus->Modbus);
            free(ptModbus);
            ptModbus = NULL;
            return NULL;
        }

        /* 响应超时时间 */
        iRet = modbus_set_response_timeout(ptModbus->Modbus, tModbusTcpCfg.TimeoutSec, tModbusTcpCfg.TimeoutUSec);
        if (iRet == -1) {
            printf("%s >> modbus_set_response_timeout failed, Err: %s\n", __func__, modbus_strerror(errno));
            modbus_free(ptModbus->Modbus);
            free(ptModbus);
            ptModbus = NULL;
            return NULL;
        }

        /* 连接从站 */
        iRet = modbus_connect(ptModbus->Modbus);
        if (iRet == -1) {
            ptModbus->IsConnect = false;
            printf("%s >> modbus_connect failed, Err: %s\n", __func__, modbus_strerror(errno));
        }
        else {
            ptModbus->IsConnect = true;
            ptModbus->IsComm = true;
        }

        ptModbus->OnConnect = modbus_connect;
        ptModbus->Onclose = modbus_close;
        ptModbus->OnReconnect = _modbus_tcp_reconnect;
        ptModbus->OnRead = _modbus_tcp_read;
        ptModbus->OnWrite = _modbus_tcp_write;
    }
    return ptModbus;
}

void modbus_tcp_master_demo(void)
{
    int           iRet = 0;
    int           iData = 100;
    TModbusTcpCfg tTable[] = {
        { MODBUS_TCP_DEV_DEMO_IP, MODBUS_TCP_DEV_DEMO_PORT, 0x01, false, 100, 0, 1000 * 300 },
    };
    TModbusTable tWriteTable[] = {
        /* 起始地址  寄存器数量  功能码 */
        { 0, 1, MODBUS_FC_WRITE_SINGLE_REGISTER },
    };
    TModbus *ptModbus = modbus_tcp_master_create(tTable[0]);
    if (ptModbus == NULL) {
        printf("ptModbus if null\n");
        return;
    }
    while (1) {
        iRet = ptModbus->OnWrite(ptModbus, tWriteTable, MODBUS_TABLE_SIZE(tWriteTable), 0, &iData);
        /* 通讯失败重连 */
        if (iRet < 0) {
            ptModbus->OnReconnect(ptModbus);
        }
        printf("%s iRet: %d, iData: %d\n", __func__, iRet, iData++);
    }
}
